﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;

namespace JobDemo01.Job
{
    class WaferAlignment(IImagingService imagingService,
    IStageService stageService,
    ImagingPipeline pipeline)
    {
        public async Task<WaferNavigatorOptions> Align(Pattern pattern)
        {
            var a = pattern.Coordinates[0];
            var b = pattern.Coordinates[1];
            var origin = await Align(a, pattern).ConfigureAwait(false);
            var dest = await Align(b, pattern).ConfigureAwait(false);
            if (a.X > b.X)
            {
                (a, b) = (b, a);
                (origin, dest) = (dest, origin);
            }

            var nav = pipeline.Navigator;
            var map = nav.Options.Map;
            var h = map.ChipH;
            double angle;
            if (nav is InterpolatedWaferNavigator lerp)
            {
                Vector v = lerp.Distance(origin, dest);
                angle = lerp.Angle(v);
                var w = v.Length / (b.X - a.X);
                if (pattern.Coordinates.Count > 2)
                {
                    nav = stageService.WaferNavigator(nav.Options with
                    {
                        VirtualScaleX = w / map.ChipW,
                        OriginX = a.X * map.ChipW + a.Dx,
                        OriginY = a.Y * map.ChipH + a.Dy,
                        StageX = origin.X,
                        StageY = origin.Y,
                        Angle = angle,
                    });
                    pipeline.Navigator = nav;

                    var c = pattern.Coordinates[2];
                    var pos = await Align(c, pattern).ConfigureAwait(false);
                    var u = lerp.Distance(origin, pos);
                    var proj = v * u / v.LengthSquared * v;
                    h = (u - proj).Length / Math.Abs(c.Y - a.Y);
                }
                return nav.Options with
                {
                    VirtualScaleX = w / map.ChipW,
                    VirtualScaleY = h / map.ChipH,
                    OriginX = a.X * map.ChipW + a.Dx,
                    OriginY = a.Y * map.ChipH + a.Dy,
                    StageX = origin.X,
                    StageY = origin.Y,
                    Angle = angle,
                };
            }
            else
            {
                var v = dest - origin;
                angle = 180 + Math.Atan2(v.Y, v.X) * 180 / Math.PI;
                angle = angle > 0 ? angle : angle + 360;
                var w = v.Length / (b.X - a.X);
                return nav.Options with
                {
                    Scale = w / map.ChipW,
                    VirtualScaleX = 1,
                    VirtualScaleY = 1,
                    OriginX = a.X * map.ChipW + a.Dx,
                    OriginY = a.Y * map.ChipH + a.Dy,
                    StageX = origin.X,
                    StageY = origin.Y,
                    Angle = angle,
                };
            }
        }

        async Task<Vector> Align(Coordinate c, Pattern pattern)
        {
            var pImg = await pipeline.MoveTo(c, pattern).ConfigureAwait(false);
            if (!pImg.ThresholdReached)
            {
                throw new AlignmentException();
            }

            var imagingSystem = imagingService.Current;
            var mat = Transformation.NewTransformation(imagingSystem.PixelSize, imagingSystem.Angle);
            var offset = mat.Transform(new Vector(-pImg.MatchDx, -pImg.MatchDy));
            var nav = pipeline.Navigator;
            var (x, y) = await nav.MotionControl.GetCoordinate().ConfigureAwait(false);
            //if (imagingService.Mode == 2)
            //{
            //    await nav.MotionControl.RelativeMove(offset.X, offset.Y);
            //    await imagingSystem.Take();
            //}
            return new Vector(x + offset.X, y + offset.Y);
        }
    }
}
